/*
 * GPSBuffer.cpp
 *
 *  Created on: Jul 7, 2012
 *      Author: jmurray
 */

#include "GPSBuffer.h"

GPSBuffer::GPSBuffer( )
{

   memset(inputBuffer,0,256);
   head=0;
   tail=0;

   state=0;

   lastUpdate = millis();

   speed=0;
   heading=0;
   lat.hour    = 0;
   lat.minute  = 0;
   lat.second  = 0;
   lon.hour    = 0;
   lon.minute  = 0;
   lon.second  = 0;
}

void GPSBuffer::stateMachine(){
   char tmp;
      tmp = Serial1.read();
      char CheckSum;

      switch(state)
      {
         case 0:
            memset(inputBuffer,0,256);
            head=0;
            tail=0;
            state=1;
            //break; // Break not needed as we should go RIGHT to state 1
            /* no break */
         case 1:
            if(tmp=='$')
            {
               state=2;
               if((head - tail) % GPSBuferSize + 1  < GPSBuferSize)
               {
                  inputBuffer[ head ] = tmp;
                  head = ( head + 1 ) % GPSBuferSize;
               }
            }
            else
            {
               // TODO Implement error case for State 1 not getting a $ char.
            }
            break;
         case 2:
            if((head - tail) % GPSBuferSize + 1 < GPSBuferSize)
            {
               inputBuffer[ head ] = tmp;
               head = ( head + 1 ) % GPSBuferSize;
            }
            else
            {
               memset(inputBuffer,0,256);
               state=1;
               break;
            }
            if(inputBuffer[(head - 1) % GPSBuferSize] != 0x0A )
               break;
            // If we have a 0x0A char (Line Feed) we need to check the string!
            state=3;
            /* no break */
         case 3:
            unsigned char tailLoc;
            tailLoc=tail;
            CheckSum=0;
            while(1)
            {
               tmp=inputBuffer[tailLoc];
               if(tmp=='$')
               {
                  tailLoc=(tailLoc + 1) % GPSBuferSize;
                  continue;
               }
               else if(tmp=='*')
                  break; // Got to end of current string!
               else if(tmp==0x00 | tailLoc==head)
               {
                  // We found a NULL somehow, we will re-init next loop!
                  // We also might have ran out of buffer or the buffer
                  //    overflowed. Reset then too!
                  state=0;
                  return;
               }
               CheckSum^=tmp;
               tailLoc=(tailLoc + 1) % GPSBuferSize;
            }
            // TODO We still need to compare the CheckSum to the last two bytes
            //    behind the tail. We should be careful that these bytes
            //    might already be stepped on by filling in NEW bytes!
            break;
         case 4:

            // TODO Call various Parse functions.
            break;
         default:
            break;
      }

}

void GPSBuffer::parseGPRMC()
{
   //TODO Get the GPRMC Function working
   return;
}


// XXX This MUST be renamed to the correct serial port!
void serialEvent1(){
   GPS.stateMachine();
}

GPSBuffer GPS;
